/*
The surface-physics project: http://code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy
License : Apache License 2.0

PhysicsEngine.cpp
Physics Engine Implementation

This file implements a simple physics engine for Orbiter as a part of the Bump
plugin for the Orbiter Flight Simulation software. Currently based off the VehicleDemo program
provided with Bullet(http://continuousphysics.com/Bullet/)

It currently updates only a single physics object : a raycast vehicle
TODO: Needs to be expanded to manage multiple objects

*/


/* Orbiter header for writing to the log */
#include "orbitersdk.h"
#include "PhysicsEngine.h"

#include "../BulletBase/Bump_BaseObject.h"



/**
 * Debugging stuff
 */
char boutput[1000]; //separate variable for threading reasons
bool cameraEnabled = true;


/**
 * Msh stuff
 * TODO these are global and will point to last msh loaded : May cause problems later
 */
int* mshIndices;
btVector3* mshVertices;
char strTerrainMshPath[100] = "Meshes\\RoughTerrain.msh"; //used by importMsh()

btRigidBody* PhysicsEngine::focusBody = NULL;


/**
 * Constructor for the engine, sets up physics and camera variables
 * Will be generalized more later with VehicleDemo stuff removed.
 */
PhysicsEngine::PhysicsEngine()
:

m_cameraHeight(4.f),
m_minCameraDistance(3.f),
m_maxCameraDistance(10.f),
m_indexVertexArrays(0),
m_vertices(0)
{
	m_cameraPosition = btVector3(30,30,30);
}


/**
 * Destructor, does nothing currently
 */
PhysicsEngine::~PhysicsEngine()
{}


/**
 * Initialize the physics world, should be empty before sim starts with commands
 * from various BulletBases to insert objects when sim starts
 * and clearing+resetting after sim ends.
 */
void	PhysicsEngine::initPhysics()
{
	//Move this outta here ! gfx stuff
	if(mode == VISUALIZE){
		setTexturing(true);
		setShadows(true);
		setCameraDistance(10.f);
	}


	//General Physics initialization
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	btVector3 worldMin(-1000,-1000,-1000);
	btVector3 worldMax(1000,1000,1000);
	m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
	m_constraintSolver = new btSequentialImpulseConstraintSolver();
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
	m_dynamicsWorld->setGravity(btVector3(0, 0, 0));


	groundShape = NULL;


}


/**
 * Free physics allocated memory
 */
void	PhysicsEngine::exitPhysics()
{

	//cleanup in the reverse order of creation/initialization

	//remove the rigidbodies from the dynamics world and delete them
	int i;
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}

	m_collisionShapes.clear();

	delete m_indexVertexArrays;
	delete m_vertices;

	//delete dynamics world
	delete m_dynamicsWorld;	

	//delete solver
	delete m_constraintSolver;

	//delete broadphase
	delete m_overlappingPairCache;

	//delete dispatcher
	delete m_dispatcher;

	delete m_collisionConfiguration;

	//Msh stuff
	delete[] mshIndices;
	delete[] mshVertices;

}


/**
 *	Called to define the type of ground by Bump_BulletBase::poststep()
 */
int
PhysicsEngine::defineGround(TerrainType terrainType)
{
	//-----Define ground : Use plane, triangle mesh, box, or .msh file for defining the ground --------
	switch(terrainType){
		case PLANE:	  groundShape = new btStaticPlaneShape(btVector3(0,1,0),0); break;
		case TRIMESH: groundShape = defineTriMeshShape(); break;
		case BOX:	  groundShape = new btBoxShape(btVector3(200, 3.0, 200)); break;
		case MSH:	  groundShape = importMsh(strTerrainMshPath); addSupportPlane(); break;
		case ORULEX:  groundShape = NULL; break;  //Ground maintained by individual vessels, nothing to be done here
		default: ;
	}

	if(groundShape){
		m_collisionShapes.push_back(groundShape);

		btTransform tr;
		tr.setIdentity();

		if(terrainType == BOX)
			tr.setOrigin(btVector3(0.f,-3.f,0.f));


		//create ground object
		localCreateRigidBody(0,tr,groundShape);
	}

	//------------------------------ground defined, now other physics objects-------------------------

	focusBody = groundRigidBody;


	return 0;
}


/**
 * Adds a mesh shape based on triangles
 * TODO: Extend to full class, generalize for different shapes
 */
btCollisionShape*
PhysicsEngine::defineTriMeshShape(void)
{
	int i;

	const float TRIANGLE_SIZE=20.f;

	//create a triangle-mesh ground
	int vertStride = sizeof(btVector3);
	int indexStride = 3*sizeof(int);

	const int NUM_VERTS_X = 10;
	const int NUM_VERTS_Y = 10;
	const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;

	const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);

	m_vertices = new btVector3[totalVerts];
	int*	gIndices = new int[totalTriangles*3];



	for ( i=0;i<NUM_VERTS_X;i++)
	{
		for (int j=0;j<NUM_VERTS_Y;j++)
		{
			float wl = .2f;
			//height set to zero, but can also use curved landscape just uncomment out the code
			float height = 0.f; //fabs(20.f*sinf(float(i)));
			m_vertices[i+j*NUM_VERTS_X].setValue(
				(i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
				height,
				(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);

		/*	printf("%d, %f, %f, %f\n", i+j*NUM_VERTS_X, m_vertices[i+j*NUM_VERTS_X].getX(),
				m_vertices[i+j*NUM_VERTS_X].getY(),
				m_vertices[i+j*NUM_VERTS_X].getZ());*/
		}
	}

	int index=0;
	for ( i=0;i<NUM_VERTS_X-1;i++)
	{
		for (int j=0;j<NUM_VERTS_Y-1;j++)
		{
			gIndices[index++] = j*NUM_VERTS_X+i;
			gIndices[index++] = j*NUM_VERTS_X+i+1;
			gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;

			gIndices[index++] = j*NUM_VERTS_X+i;
			gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
			gIndices[index++] = (j+1)*NUM_VERTS_X+i;
		}
	}

	/*for(i=0; i<totalTriangles*3; i++)
		printf(" gIndices[%d] : %d\n", i, gIndices[i]); */

	m_indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
		gIndices,
		indexStride,
		totalVerts,(btScalar*) &m_vertices[0].x(),vertStride);

	bool useQuantizedAabbCompression = true;
	return new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression);

}


/**
 * Adds an Orbiter .msh format mesh to the physics world by reading it from a .msh file
 * Converts the triangle list in .msh -> btBvhTriangleMeshShape
 *
 */
btCollisionShape*
PhysicsEngine::importMsh(char *fileName)
{
	int i, g, numGroups = 0, materialIndex=0, textureIndex=0;
	int *nv, *nt;
	float x, y, z, nx, ny, nz;
	char buffer[50], label[50];
	FILE* fp;
	int **mshIndicesList;
	btVector3 **mshVerticesList;
	int totalVertices = 0, totalIndices = 0;

	//create a triangle-mesh
	int vertStride = sizeof(btVector3);
	int indexStride = 3*sizeof(int);
	const float TRIANGLE_SIZE=5.f;

	printf("\n-------Reading msh file------\n");

	fopen_s(&fp, fileName,"r");
	if(fp == NULL){
		/*sprintf(boutput, "The MSH file \"%s\" could not be opened. Is it being held by another application ?",
				fileName); oapiWriteLog(boutput);
		return NULL;*/
	}

	fscanf_s(fp, "MSHX1\n");
	fscanf_s(fp, "GROUPS %d\n", &numGroups);
	printf("Number of groups = %d\n", numGroups);
	mshVerticesList = new btVector3*[numGroups];
	mshIndicesList  = new int*[numGroups];
	nv  = new int[numGroups];
	nt  = new int[numGroups];

	//Read in the vertices of all groups
	for(g=0; g<numGroups; g++){
		fscanf_s(fp, "LABEL ", buffer);
		fgets(label, 49, fp);
		fscanf_s(fp, "MATERIAL %d\n", &materialIndex);
		fscanf_s(fp, "TEXTURE %d\n", &textureIndex);
		fscanf_s(fp, "GEOM %d %d ;", &nv[g], &nt[g]);
		fgets(buffer, 49, fp);
		printf("------------Group = %d(Ignoring normals, materials, textures)-------------\n", g);
		printf("Label = %s", label);
		printf("Material Index = %d\n", materialIndex);
		printf("Texture Index = %d\n", textureIndex);
		printf("Number of vertices=%d, Number of Triangles=%d\n", nv[g], nt[g]);

		//Read in the vertices & normals for the current group in separate arrays
		mshVerticesList[g] = new btVector3[nv[g]];
		for(i=0; i<nv[g]; i++){
			fscanf_s(fp, "%f %f %f %f %f %f\n", &x,  &y,  &z, &nx, &ny, &nz);
			mshVerticesList[g][i].setValue(x, y, z);
		}

		//Read in the triangle indices for the current group in separate arrays BUT
		//offset the current group's indices by previously read total indices to have uniform triangle information
		mshIndicesList[g] = new int[nt[g]*3];
		for(i=0; i<nt[g]*3; i+=3)
			fscanf_s(fp, "%d %d %d\n", &mshIndicesList[g][i], &mshIndicesList[g][i+1], &mshIndicesList[g][i+2]);

	/*	for(i=0; i<nv[g]; i++)
			printf("[%d] %f %f %f \n", i,
			mshVerticesList[g][i].getX(),  mshVerticesList[g][i].getY(), mshVerticesList[g][i].getZ());

		for(i=0; i<nt[g]*3; i+=3)
			printf("[%d] %d %d %d\n", i, mshIndicesList[g][i], mshIndicesList[g][i+1], mshIndicesList[g][i+2]);*/

		totalVertices += nv[g];
		totalIndices += (nt[g]*3); //number of indices is no. of triangles * 3

		printf("Total Vertices so far  : %d\n", totalVertices);
		printf("Total Indices so far  : %d\n", totalIndices);
	}

	//Merge vertices and indices list into 1
	mshVertices = new btVector3[totalVertices];
	mshIndices  = new int[totalIndices];

	int vertexOffset = 0, indexOffset = 0;
	for(g=0; g<numGroups; g++){
		//Vertices copy
		for(i=0; i<nv[g]; i++){
			mshVertices[vertexOffset + i] = mshVerticesList[g][i];
		}

		//Indices copy
		for(i=0; i<nt[g]*3; i++){
			mshIndices[indexOffset + i] = mshIndicesList[g][i] + vertexOffset; //add offset of already added total vertices
		}

		vertexOffset += nv[g]; //important to do this last(see addition above)
		indexOffset += nt[g]*3;
	}

	printf("-----Merge Complete of %d vertices and %d indices-----\n", totalVertices, totalIndices);
/*	for(i=0; i<totalVertices; i++)
			printf("[%d] %f %f %f \n", i, mshVertices[i].getX(),  mshVertices[i].getY(), mshVertices[i].getZ());

	for(i=0; i<totalIndices; i+=3)
		printf("[%d] %d %d %d\n", i, mshIndices[i], mshIndices[i+1], mshIndices[i+2]);

*/
	//Finally make the triangle mesh
	m_indexVertexArrays = new btTriangleIndexVertexArray(totalIndices/3,
			mshIndices,
			indexStride,
			totalVertices,(btScalar*) &mshVertices[0].x(),vertStride);

	bool useQuantizedAabbCompression = true;
	btCollisionShape* p = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression);

	fclose(fp);

	printf("\n-------Mesh read complete------\n\n");

	delete[] mshVerticesList;
	delete[] mshIndicesList;
	delete nv;
	delete nt;

	return p;
}


/**
 * Wrapper around importMsh() to easily test .msh based triangle meshes
 * Converts the btBvhTriangleMeshShape -> btRigidBody
 * This is for dynamic bodies(triangle meshes not recommended for dynamic objects)
 */
btRigidBody*
PhysicsEngine::addOrbiterMshDynamic(char *fileName, btVector3& offset)
{
	btCollisionShape* p = importMsh(fileName);
	m_collisionShapes.push_back(p);
	btTransform trans(btQuaternion(btVector3(0,1,0), (const btScalar)PI/2), offset);

	btDefaultMotionState* mshMotionState = new btDefaultMotionState(trans);
    btRigidBody::btRigidBodyConstructionInfo mshRigidBodyCI(0, mshMotionState, p, btVector3(0,0,0));
    btRigidBody* mshRigidBody = new btRigidBody(mshRigidBodyCI);
    m_dynamicsWorld->addRigidBody(mshRigidBody);

    return mshRigidBody;
}


/**
 * Wrapper around importMsh() to easily test .msh based triangle meshes
 * Converts the btBvhTriangleMeshShape -> btRigidBody
 * This is for static geometry
 */
btRigidBody*
PhysicsEngine::addOrbiterMshStatic(char *fileName, btVector3& offset)
{
	btCollisionShape* collShpMSH = importMsh(fileName);
	m_collisionShapes.push_back(collShpMSH);
	btTransform transMSH(btQuaternion(btVector3(0,1,0), (const btScalar)0), offset);

	return localCreateRigidBody(0.f, transMSH, collShpMSH);
}


/**
 * Defines a supporting plane to prevent vehicles from falling over the edge for
 * non-infinite ground shapes. This is more of an utility function and need not be made into a class
 */
void PhysicsEngine::addSupportPlane(void)
{
	supportPlaneShape = new btStaticPlaneShape(btVector3(0,1,0),0);
	m_collisionShapes.push_back(supportPlaneShape);
	btDefaultMotionState* supportMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),
																		btVector3(0,0,0)));
    btRigidBody::btRigidBodyConstructionInfo supportRigidBodyCI(0, supportMotionState,
    															   supportPlaneShape,
    															   btVector3(0,0,0));
    btRigidBody* supportRigidBody = new btRigidBody(supportRigidBodyCI);
    m_dynamicsWorld->addRigidBody(supportRigidBody);
}


/**
 * GLUT callback  function to update physics, in the absence of GLUT will be called
 * directly from Bump to update the physics.
 */
void PhysicsEngine::clientMoveAndDisplay(void)
{
	float dt;
	
	//dt = getDeltaTimeMicroseconds() * 0.000001f;
	dt = (float)oapiGetSimStep();

	if (m_dynamicsWorld)
	{
		//during idle mode, just run 1 simulation step maximum
		int maxSimSubSteps = m_idle ? 1 : 2;
		if (m_idle)
			dt = 1.0/420.f;

		int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);


	}

		

	//Gfx render
	//glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);


	//renderme();

	//optional but useful: debug drawing
	/*if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();*/


	//glFlush();
	//swapBuffers();
	

}


/**
 * Used to reset the scene. This may not be a good idea when the engine is expanded
 * to dynamically add/remove objects
 * TODO: Figure out when reset makes sense, for the moment with a single car it does.
 */
void	PhysicsEngine::clientResetScene()
{
	exitPhysics();
	initPhysics();
}


/**
 * Used in visualizer mode to render, by GLUT
 */
void PhysicsEngine::displayCallback(void)
{

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	
	renderme();

	//optional but useful: debug drawing to detect problems
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();

	glFlush();
	swapBuffers();
}


/**
 * Added by Bump, to detect window close
 */
void PhysicsEngine::WMCloseFuncCallback(void)
{

}


/**
 * Called for special keys, when a key is released. Only used with GLUT
 */
void PhysicsEngine::specialKeyboardUp(int key, int x, int y)
{
   switch (key)
    {
/*    case GLUT_KEY_UP :
		{
			gEngineForce = 0.f;
		break;
		}
	case GLUT_KEY_DOWN :
		{
			gBreakingForce = 0.f;
		break;
		}
*/
   case 1:
   default:
		DemoApplication::specialKeyboardUp(key,x,y);
        break;
    }

}


/**
 * Called for special keys, when a key is released. Only used with GLUT
 */
void PhysicsEngine::specialKeyboard(int key, int x, int y)
{

//	printf("key = %i x=%i y=%i\n",key,x,y);

    switch (key)
    {
/*    case GLUT_KEY_LEFT :
		{
			gVehicleSteering += steeringIncrement;
			if (	gVehicleSteering > steeringClamp)
					gVehicleSteering = steeringClamp;

		break;
		}
    case GLUT_KEY_RIGHT :
		{
			gVehicleSteering -= steeringIncrement;
			if (	gVehicleSteering < -steeringClamp)
					gVehicleSteering = -steeringClamp;

		break;
		}
    case GLUT_KEY_UP :
		{
			m_carChassis->setActivationState(DISABLE_DEACTIVATION);
			gEngineForce = maxEngineForce;
			gBreakingForce = 0.f;
		break;
		}
	case GLUT_KEY_DOWN :
		{
			gBreakingForce = maxBreakingForce;
			gEngineForce = 0.f;
		break;
		}
*/
	case GLUT_KEY_F10 :
		{
			cameraEnabled = !cameraEnabled;
		break;
		}


	case 1:
	default:
		DemoApplication::specialKeyboard(key,x,y);
        break;
    }

}


/**
 * Updates the camera position
 * TODO: Needs to be updated to work with multiple objects and
 * BulletBases.
 */
void PhysicsEngine::updateCamera()
{

	if(cameraEnabled){
		DemoApplication::updateCamera();
		return;
	}

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();

	btTransform transFocusBody;

	//look at this body

	//focusBody->getMotionState()->getWorldTransform(groundWorldTrans);
	
	if(focusBody){
		btMotionState *msFocusBody = focusBody->getMotionState();
		if(msFocusBody){
			msFocusBody->getWorldTransform(transFocusBody);
			m_cameraTargetPosition = transFocusBody.getOrigin();
		}
		else{
			transFocusBody = focusBody->getCenterOfMassTransform();
			m_cameraTargetPosition = transFocusBody.getOrigin();
		}


	//interpolate the camera height
#ifdef FORCE_ZAXIS_UP
		m_cameraPosition[2] = (15.0f *m_cameraPosition[2] + m_cameraTargetPosition[2] + m_cameraHeight)/16.0f;
#else
		m_cameraPosition[1] = (15.0f *m_cameraPosition[1] + m_cameraTargetPosition[1] + m_cameraHeight)/16.0f;
#endif

		btVector3 camToObject = m_cameraTargetPosition - m_cameraPosition;

		//keep distance between min and max distance
		float cameraDistance = camToObject.length();
		float correctionFactor = 0.f;
		if (cameraDistance < m_minCameraDistance)
		{
			correctionFactor = 0.15f *(m_minCameraDistance-cameraDistance)/cameraDistance;
		}
		if (cameraDistance > m_maxCameraDistance)
		{
			correctionFactor = 0.15f *(m_maxCameraDistance-cameraDistance)/cameraDistance;
		}
		m_cameraPosition -= correctionFactor*camToObject;

		  btScalar aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
			glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);

			 glMatrixMode(GL_MODELVIEW);
			 glLoadIdentity();

		gluLookAt(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2],
				  m_cameraTargetPosition[0],m_cameraTargetPosition[1], m_cameraTargetPosition[2],
				  m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
	}



}


/**
 * Used to render wheels and stuff, useful for rendering stuff
 * not rendered by DemoApplication:renderme()
 * Also updates camera
 *
 * TODO: External mesh rendering which are not a part of the dynamics world like wheels
 */
void PhysicsEngine::renderme()
{

	updateCamera();


	btVector3 wheelColor(1,0,0), collisionColor(0.5,0.5,1);

	btVector3 worldBoundsMin, worldBoundsMax;
	getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);



	//External rendering functions for wheels should be inserted here
	for(renderObjIter = renderObjList.begin();  renderObjIter != renderObjList.end(); renderObjIter++){
		if((*renderObjIter))
			(*renderObjIter)->renderme();
	}


	DemoApplication::renderme();

} 

